#include "DAEFloat3.h"
#include "math.h"


DAEFloat3::DAEFloat3():x(0),y(0),z(0)
{
}

DAEFloat3::DAEFloat3(float xValue,float yValue, float zValue):x(xValue),y(yValue),z(zValue)
{
}

DAEFloat3::DAEFloat3(float xValue, float yValue, float zValue,bool normalize):x(xValue),y(yValue),z(zValue)
{
	if (normalize)
		Normalize();
}

void DAEFloat3::Normalize(){
	float length= sqrt( x*x+y*y+z*z);
	x/=length;
	y/=length;
	z/=length;
}

DAEFloat3& DAEFloat3::operator=(const DAEFloat3& float3)
{
	this->x=float3.x;
	this->y=float3.y;
	this->z=float3.z;
	return *this;
}

DAEFloat3::DAEFloat3(const DAEFloat3& float3)
{
	this->x=float3.x;
	this->y=float3.y;
	this->z=float3.z;
}

DAEFloat3& DAEFloat3::Add(DAEFloat3& outResult, const float scale, const DAEFloat3& vector) const
{
	float scalex = scale*vector.x;
	float scaley = scale*vector.y;
	float scalez = scale*vector.z;

	float rx = x + scalex;
	float ry = y + scaley;
	float rz = z + scalez;

	outResult.SetValues(rx,ry,rz);
	return outResult;	
}


DAEFloat3& DAEFloat3::Add(DAEFloat3& outResult,const DAEFloat3& position) const
{
	outResult.SetValues(x+position.x, y+position.y,z+position.z);
	return outResult;
}

DAEFloat3& DAEFloat3::Subtract(DAEFloat3& outResult,const DAEFloat3& position) const
{
	outResult.SetValues(x-position.x,y-position.y,z-position.z);
	return outResult;
}

DAEFloat3& DAEFloat3::CreateVector(DAEFloat3& outResult,const DAEFloat3& to) const
{
	float xr = to.x - x;
	float yr = to.y - y;
	float zr = to.z - z;
	outResult.SetValues(xr,yr,zr);
	return outResult;
}

void DAEFloat3::CreatePerpendicularVector(DAEFloat3& outResult) const
{
	DAEFloat3 frame;
	float absx = abs(x);
	float absy = abs(y);
	float absz = abs(z);
	if ( absx < absy &&absx < absz ){
		frame.SetValues(1,0,0);
	}else if ( absy < absx && absy < absz ){
		frame.SetValues(0,1,0);
	}else
		frame.SetValues(0,0,1);

	this->CrossAndNormalize(outResult,frame);
}

DAEFloat3& DAEFloat3::Cross(DAEFloat3& outResult,const DAEFloat3& vector) const
{
	float xr = this->y * vector.z - this->z * vector.y;
	float yr = this->z * vector.x - this->x * vector.z;
	float zr = this->x * vector.y - this->y * vector.x;
	outResult.SetValues(xr,yr,zr);
	return outResult;
}


DAEFloat3& DAEFloat3::CrossAndNormalize(DAEFloat3& outResult, const DAEFloat3& vector) const
{
	// a = b cross c
	// b = this
	// c = vector
	//a_x = b_y * c_z - b_z * c_y
    //a_y = b_z * c_x - b_x * c_z 
    //a_z = b_x * c_y - b_y * c_x 
	float xr = this->y * vector.z - this->z * vector.y;
	float yr = this->z * vector.x - this->x * vector.z;
	float zr = this->x * vector.y - this->y * vector.x;
	outResult.SetValues(xr,yr,zr);
	outResult.Normalize();
	return outResult;

}

float DAEFloat3::Dot(const DAEFloat3 & vector2) const
{
	return this->x*vector2.x + this->y*vector2.y + this->z*vector2.z;
}

DAEFloat3& DAEFloat3::Scale(DAEFloat3& outResult, const float scale) const
{
	outResult.SetValues(x*scale,y*scale,z*scale);
	return outResult;
}

void DAEFloat3::Clamp(float min, float max)
{
	if ( x > max )
		x = max;
	else if ( x < min)
		x = min;

	if ( y > max )
		y = max;
	else if ( y < min)
		y = min;

	if ( z > max )
		z = max;
	else if ( z < min)
		z = min;
}

DAEFloat3 DAEFloat3::BarycentricCoordinates(DAEFloat3& p, DAEFloat3& p1, DAEFloat3& p2, DAEFloat3& p3)
{
	DAEFloat3 cross;
	DAEFloat3 e13 = p1-p3;
	DAEFloat3 e23 = p2-p3;
	DAEFloat3 ep3 = p-p3;
	e13.Cross(cross,e23);
	
	float ax = abs(cross.x);
	float ay = abs(cross.y);
	float az = abs(cross.z);

	DAEFloat3 result;

	if ( az > ax && az > ay )
	{
		// use xy plane.
		// l1
		result.x = (e23.y*ep3.x - e23.x*ep3.y) / (e23.y * e13.x - e23.x *e13.y);
		result.y = (-e13.y*ep3.x + e13.x*ep3.y) / (e23.y * e13.x - e23.x *e13.y);
		result.z = 1-result.x-result.y;
	}else if ( ay > ax && ay > az ){
		// use xz plane
		result.x = (e23.z*ep3.x - e23.x*ep3.z) / (e23.z * e13.x - e23.x *e13.z);
		result.y = (-e13.z*ep3.x + e13.x*ep3.z) / (e23.z * e13.x - e23.x *e13.z);
		result.z = 1-result.x-result.y;
	}else{
		result.x = (e23.z*ep3.y - e23.y*ep3.z) / (e23.z * e13.y - e23.y *e13.z);
		result.y = (-e13.z*ep3.y + e13.y*ep3.z) / (e23.z * e13.y - e23.y *e13.z);
		result.z = 1-result.y - result.x;
	}
	return result;
}